using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
//using Poly2Tri;

namespace GameStateManagementSample.Classes
{
    public class GeometryHelper
    {
        public static Vector2? LinesIntersect(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4)
        {
            float A1 = p2.Y - p1.Y;
            float B1 = p1.X - p2.X;
            float C1 = p1.X * p2.Y - p2.X * p1.Y;

            float A2 = p4.Y - p3.Y;
            float B2 = p3.X - p4.X;
            float C2 = p3.X * p4.Y - p4.X * p3.Y;

            float delta = A1 * B2 - A2 * B1;
            if (delta == 0) return null;

            float x = (B2 * C1 - B1 * C2) / delta;
            float y = (A1 * C2 - A2 * C1) / delta;

            if (
                (Math.Min(p1.X, p2.X) <= x) && (x <= Math.Max(p1.X, p2.X)) &&
                (Math.Min(p1.Y, p2.Y) <= y) && (y <= Math.Max(p1.Y, p2.Y)) &&
                (Math.Min(p3.X, p4.X) <= x) && (x <= Math.Max(p3.X, p4.X)) &&
                (Math.Min(p3.Y, p4.Y) <= y) && (y <= Math.Max(p3.Y, p4.Y))
                )
            {
                return new Vector2(x, y);
            }
            else
            {
                return null;
            }

        }

        public static Vector2? GetTrajectoryIntersectionOnLine(Vector2 l1, Vector2 l2, Vector2 start, Vector2 end)
        {
            float A1 = l2.Y - l1.Y;
            float B1 = l1.X - l2.X;
            float C1 = l1.X * l2.Y - l2.X * l1.Y;

            float A2 = end.Y - start.Y;
            float B2 = start.X - end.X;
            float C2 = start.X * end.Y - end.X * start.Y;

            float delta = A1 * B2 - A2 * B1;
            if (delta == 0) return null;

            float x = (B2 * C1 - B1 * C2) / delta;
            float y = (A1 * C2 - A2 * C1) / delta;

            if (
                (Math.Min(l1.X, l2.X) <= x) && (x <= Math.Max(l1.X, l2.X)) &&
                (Math.Min(l1.Y, l2.Y) <= y) && (y <= Math.Max(l1.Y, l2.Y)) &&
                (((start.X < end.X) && (x > start.X)) || ((start.X > end.X) && (x < start.X))) &&
                (((start.Y < end.Y) && (y > start.Y)) || ((start.Y > end.Y) && (y < start.Y)))
                //(start.X <= x) && (x <= start.X) &&
                //(start.Y <= y) && (y <= start.Y)
                )
            {
                return new Vector2(x, y);
            }
            else
            {
                return null;
            }

        }

        //public static Vector2? GetTrajectoryIntersectionOnLineDouble(Vector2 l1, Vector2 l2, Vector2 start, Vector2 end)
        //{
        //    double A1 = l2.Y - l1.Y;
        //    double B1 = l1.X - l2.X;
        //    double C1 = l1.X * l2.Y - l2.X * l1.Y;

        //    double A2 = end.Y - start.Y;
        //    double B2 = start.X - end.X;
        //    double C2 = start.X * end.Y - end.X * start.Y;

        //    double delta = A1 * B2 - A2 * B1;
        //    if (delta == 0) return null;

        //    double x = (B2 * C1 - B1 * C2) / delta;
        //    double y = (A1 * C2 - A2 * C1) / delta;

        //    if (
        //        (Math.Min(l1.X, l2.X) <= x) && (x <= Math.Max(l1.X, l2.X)) &&
        //        (Math.Min(l1.Y, l2.Y) <= y) && (y <= Math.Max(l1.Y, l2.Y)) &&
        //        (((start.X < end.X) && (x > start.X)) || ((start.X > end.X) && (x < start.X))) &&
        //        (((start.Y < end.Y) && (y > start.Y)) || ((start.Y > end.Y) && (y < start.Y)))
        //        //(start.X <= x) && (x <= start.X) &&
        //        //(start.Y <= y) && (y <= start.Y)
        //        )
        //    {
        //        return new Vector2((float)x, (float)y);
        //    }
        //    else
        //    {
        //        return null;
        //    }

        //}
        public static Vector2 GetClosestPointOnLine(Vector2 A, Vector2 B, Vector2 P)
        {
            Vector2 AP = P - A;       //Vector from A to P   
            Vector2 AB = B - A;       //Vector from A to B  

            float magnitudeAB = AB.LengthSquared();     //Magnitude of AB vector (it's length squared)     
            float ABAPproduct = Vector2.Dot(AP, AB);    //The DOT product of a_to_p and a_to_b     
            float distance = ABAPproduct / magnitudeAB; //The normalized "distance" from a to your closest point  

            if (distance < 0)     //Check if P projection is over vectorAB     
            {
                return A;

            }
            else if (distance > 1)
            {
                return B;
            }
            else
            {
                return A + AB * distance;
            }
        }

        public static Vector2 GetClosestPointOnAreaBorder(Vector2 P, List<Vector2> poly)
        {

            if (poly.Count == 0) return -Vector2.One;
            //var poly = Vectors;

            Vector2 last = poly.Last();
            var distance = Vector2.Distance(P, poly.First());
            Vector2 retPnt = poly.First();
            foreach (var vert in poly)
            {
                var pnt = GeometryHelper.GetClosestPointOnLine(last, vert, P);
                var tmpdist = Vector2.Distance(P, pnt);
                if (tmpdist < distance) { distance = tmpdist; retPnt = pnt; }
                last = vert;
            }
            return retPnt;
        }

        public static Vector2 GetLinePointAt(float t, Vector2 start, Vector2 end)
        {
            /* Formula from Wikipedia article on Bezier curves. */
            float X = (start.X + end.X) * t;
            float Y = (start.Y + end.Y) * t;
            return new Vector2(X, Y);
        }



        public static float GetPolyArea(IList<Vector2> pts)
        {
            float area = 0;
            int len = pts.Count;
            for (var i = 0; i < len; ++i)
            {
                Vector2 p1 = pts[i], p2 = pts[(i + len - 1) % len];
                area += (p2.X + p1.X) * (p2.Y - p1.Y);
            }
            return Math.Abs(area / 2);
        }
        public static Vector2 LineMiddle(Vector2 p1, Vector2 p2)
        {
            return new Vector2((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2);
        }


        public static Vector2 AngleToVector(float angle)
        {
            return new Vector2((float)Math.Sin(angle), -(float)Math.Cos(angle));
        }

        public static float VectorToAngle(Vector2 vector)
        {
            return (float)Math.Atan2(vector.X, -vector.Y);
        }
        public static bool PointIsOnLine(Vector2 endPoint1, Vector2 endPoint2, Vector2 checkPoint)
        {
            if (Vector2.Distance(GetClosestPointOnLine(endPoint1, endPoint2, checkPoint), checkPoint) < 0.0001f)
                return true;
            else
                return false;
        }
        public static bool PointIsOnLine_(Vector2 endPoint1, Vector2 endPoint2, Vector2 checkPoint)
        {
            Vector2 p1 = new Vector2(201.497009277344f, 298.91162109375f);
            Vector2 p2 = new Vector2(167.77001953125f, 301.117309570312f);
            Vector2 chk = new Vector2(430f, 285f);


            //If the point is on the line then:
            //(x - x1) / (x2 - x1) = (y - y1) / (y2 - y1) = (z - z1) / (z2 - z1)
            //Calculate all three values, and if they are the same (to some degree of tolerance), your point is on the line.
            //To test if the point is in the segment, not just on the line, you can check that x1 < x < x2, assuming x1 < x2 (or y1 < y < y2, or z1 < z < z2)


            //if (System.Math.Round(checkPoint.X - endPoint1.X, 2) == 0 && System.Math.Round(endPoint2.X - endPoint1.X, 2) == 0)
            //{
            //   // return (checkPoint.Y >= Math.Min(endPoint1.Y, endPoint2.Y)) && (checkPoint.Y <= Math.Max(endPoint1.Y, endPoint2.Y));
            //    return System.Math.Round(checkPoint.Y - Math.Min(endPoint1.Y, endPoint2.Y), 2) >= 0
            //        && System.Math.Round(checkPoint.Y - Math.Max(endPoint1.Y, endPoint2.Y), 2) <= 0;
            //}
            //else 
            if (System.Math.Round(endPoint2.Y - endPoint1.Y, 2) == 0)
            {
                return (System.Math.Round(checkPoint.Y - endPoint1.Y, 2) == 0 && Math.Min(endPoint1.X, endPoint2.X) <= checkPoint.X) && (checkPoint.X <= Math.Max(endPoint1.X, endPoint2.X));
            }
            else if (System.Math.Round(endPoint2.X - endPoint1.X, 2) == 0)
            {
                return (System.Math.Round(checkPoint.X - endPoint1.X, 2) == 0 && Math.Min(endPoint1.Y, endPoint2.Y) <= checkPoint.Y) && (checkPoint.Y <= Math.Max(endPoint1.Y, endPoint2.Y));
            }

            if (System.Math.Round(((checkPoint.Y - endPoint1.Y) / (endPoint2.Y - endPoint1.Y))
                  - ((checkPoint.X - endPoint1.X) / (endPoint2.X - endPoint1.X)), 2) == 0)
            {
                return (
                    (Math.Min(endPoint1.X, endPoint2.X) < checkPoint.X) && (checkPoint.X < Math.Max(endPoint1.X, endPoint2.X)) &&
                    (Math.Min(endPoint1.Y, endPoint2.Y) < checkPoint.Y) && (checkPoint.Y < Math.Max(endPoint1.Y, endPoint2.Y))
                    );
            }
            return false;
        }


        public static bool IsClockwisePolygon(List<Vector2> polygon)
        {
            bool isClockwise = false;

            double sum = 0;

            Vector2 last = polygon.Last();
            foreach (var vect in polygon)
            {
                //LineBatchTester.LineBatch.DrawLine(spriteBatch, Color.Red, vect, last);
                sum += (vect.X - last.X) * (vect.Y + last.Y);
                last = vect;
            }
            isClockwise = (sum > 0) ? true : false;
            return !isClockwise;

        }

        public static Vector2 Compute2DPolygonCentroid(List<Vector2> vertices)
        {
            Vector2 centroid = new Vector2() { X = 0.0f, Y = 0.0f };
            float signedArea = 0.0f;
            float x0 = 0.0f; // Current vertex X
            float y0 = 0.0f; // Current vertex Y
            float x1 = 0.0f; // Next vertex X
            float y1 = 0.0f; // Next vertex Y
            float a = 0.0f;  // Partial signed area

            // For all vertices except last
            int i = 0;
            for (i = 0; i < vertices.Count - 1; ++i)
            {
                x0 = vertices[i].X;
                y0 = vertices[i].Y;
                x1 = vertices[i + 1].X;
                y1 = vertices[i + 1].Y;
                a = x0 * y1 - x1 * y0;
                signedArea += a;
                centroid.X += (x0 + x1) * a;
                centroid.Y += (y0 + y1) * a;
            }

            // Do last vertex
            x0 = vertices[i].X;
            y0 = vertices[i].Y;
            x1 = vertices[0].X;
            y1 = vertices[0].Y;
            a = x0 * y1 - x1 * y0;
            signedArea += a;
            centroid.X += (x0 + x1) * a;
            centroid.Y += (y0 + y1) * a;

            signedArea *= 0.5f;
            centroid.X /= (6 * signedArea);
            centroid.Y /= (6 * signedArea);

            return centroid;
        }
        public static Vector2 getPointAtDistance(float t, Vector2 p1, Vector2 p2)
        {
            //get vector   
            //var x3: = p2.X - p1.X;
            //var y3 = p2.Y - p1.Y;
            var v3 = (p2 - p1) * t;
            ////normalize vector
            //// var  = Math.sqrt( x3 * x3 + y3 * y3 );
            ////x3 /= length;
            ////y3 /= length;
            //v3 = Vector2.Normalize(v3);
            ////scale vector
            //v3 *= t;
            ////x3 *= dis;
            ////y3 *= dis;

            //add vector back onto initial point and return
            return p1 + v3;

        }

        public static bool TestLinesIntersectExclusive(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4)
        {
            float ua = (point4.X - point3.X) * (point1.Y - point3.Y) - (point4.Y - point3.Y) * (point1.X - point3.X);
            float ub = (point2.X - point1.X) * (point1.Y - point3.Y) - (point2.Y - point1.Y) * (point1.X - point3.X);
            float denominator = (point4.Y - point3.Y) * (point2.X - point1.X) - (point4.X - point3.X) * (point2.Y - point1.Y);

            bool intersection = false, coincident = false;
            
            if (Math.Abs(denominator) <= 0.00001f)
            {
                if (Math.Abs(ua) < 0.00001f && Math.Abs(ub) < 0.00001f)
                {
                    //intersection = coincident = true;
                    //intersectionPoint = (point1 + point2) / 2;
                    return true;
                }
            }
            else
            {
                ua /= denominator;
                ub /= denominator;

                if (ua > 0 && ua < 1 && ub > 0 && ub < 1)
                {

                    //intersection = true;
                    Vector2 intersectionPoint = new Vector2();
                    intersectionPoint.X = point1.X + ua * (point2.X - point1.X);
                    intersectionPoint.Y = point1.Y + ua * (point2.Y - point1.Y);
                    if ((Vector2.Distance(point1, intersectionPoint) < 0.001f) ||
                        (Vector2.Distance(point2, intersectionPoint) < 0.001f) ||
                        (Vector2.Distance(point3, intersectionPoint) < 0.001f) ||
                        (Vector2.Distance(point4, intersectionPoint) < 0.001f))
                        return false;
                    else
                        return true;
                }
            }
            return false;
        }

        public static bool TestLinesIntersectInclusive(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4)
        {
            float ua = (point4.X - point3.X) * (point1.Y - point3.Y) - (point4.Y - point3.Y) * (point1.X - point3.X);
            float ub = (point2.X - point1.X) * (point1.Y - point3.Y) - (point2.Y - point1.Y) * (point1.X - point3.X);
            float denominator = (point4.Y - point3.Y) * (point2.X - point1.X) - (point4.X - point3.X) * (point2.Y - point1.Y);

            bool intersection = false, coincident = false;

            if (Math.Abs(denominator) <= 0.00001f)
            {
                if (Math.Abs(ua) <= 0.00001f && Math.Abs(ub) <= 0.00001f)
                {
                    intersection = coincident = true;
                    // intersectionPoint = (point1 + point2) / 2;
                }
            }
            else
            {
                ua /= denominator;
                ub /= denominator;

                if (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1)
                {
                    intersection = true;
                    //   intersectionPoint.X = point1.X + ua * (point2.X - point1.X);
                    //   intersectionPoint.Y = point1.Y + ua * (point2.Y - point1.Y);
                }
            }
            return intersection;
        }

        public static Vector2? GetTrajectoryIntersectionOnLineDouble(Vector2 l1, Vector2 l2, Vector2 start, Vector2 end)
        {
            double A1 = l2.Y - l1.Y;
            double B1 = l1.X - l2.X;
            double C1 = l1.X * l2.Y - l2.X * l1.Y;

            double A2 = end.Y - start.Y;
            double B2 = start.X - end.X;
            double C2 = start.X * end.Y - end.X * start.Y;

            double delta = A1 * B2 - A2 * B1;
            if (delta == 0) return null;

            double x = (B2 * C1 - B1 * C2) / delta;
            double y = (A1 * C2 - A2 * C1) / delta;

            if (
                (Math.Round(Math.Min(l1.X, l2.X) - x, 2) <= 0) && (0 <= Math.Max(l1.X, l2.X) - x) &&
                (Math.Round(Math.Min(l1.Y, l2.Y) - y, 2) <= 0) && (0 <= Math.Max(l1.Y, l2.Y) - y) &&
                (((start.X < end.X) && (x > start.X)) || ((start.X > end.X) && (x < start.X))) &&
                (((start.Y < end.Y) && (y > start.Y)) || ((start.Y > end.Y) && (y < start.Y)))
                //(start.X <= x) && (x <= start.X) &&
                //(start.Y <= y) && (y <= start.Y)
                )
            {
                return new Vector2((float)x, (float)y);
            }
            else
            {
                return null;
            }

        }
        public static float DegreeToRadian(float angle)
        {
            return (float)Math.PI * angle / 180;
        }
        public static double RadianToDegree(double angle)
        {
            return angle * (180.0 / Math.PI);
        }


    }
}
